﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO.Ports;

namespace CarController
{
    class Pololu
    {
        SerialPort serial;

        public Pololu(string ComPort)
        {
            try
            {
                serial = new SerialPort(ComPort, 38400, Parity.None, 8, StopBits.One);
                serial.Open();
            }
            catch (Exception e)
            {
                Console.WriteLine("There is no COM port!");
            }
        }

        /// <summary>
        /// Set the absolute servo position.
        /// </summary>
        /// <param name="position">Position value between 0 and 1</param>
        private void SetPosition(int servo, double position)
        {
            int value = (int)((position * 5000) + 500);
            byte[] data = new byte[] { 0x80, 0x01, 0x04, (byte)servo, 0x00, 0x00 };

            data[5] = (byte)(value & 0x7f);
            data[4] = (byte)((value >> 7) & 0x7f);
            serial.Write(data, 0, 6);
        }

        public void Kill()
        {
            int servoCount = 8;
            for (int i = 0; i < servoCount; i++)
            {
                byte[] data = new byte[] { 0x80, 0x01, 0x00, (byte)i, 0x00 };
                serial.Write(data, 0, 5);
            }
        }

        /// <summary>
        /// Set the steering position.
        /// </summary>
        /// <param name="position">Position value between -1 (full left lock) and 1 (full right lock).</param>
        public void SetSteering(double position)
        {

            position = Math.Max(position, -1);
            position = Math.Min(position, 1);

            double panPosition = position;

            position = -position;

            position *= 0.3;
            position += 0.37;

            SetPosition(0, position);

            panPosition *= 0.25;
            SetPan(panPosition);
            SetTilt(speedTilt);
        }

        double speedTilt = -0.35;
        /// <summary>
        /// Set the speed of the vehicle.
        /// </summary>
        /// <param name="speed">Speed between -1 (full speed reverse) and 1 (full speed forwards).</param>
        public void SetSpeed(double speed)
        {
            speedTilt = Math.Max(0, speed - 0.3) * 0.35 - 0.35;
            
            speed = -speed;

            SetPosition(1, (speed * 0.25) + 0.5);
            
            SetTilt(speedTilt);
        }

        public void Stop()
        {
            SetSpeed(0);
        }

        /// <summary>
        /// Set the pan of the camera.
        /// </summary>
        /// <param name="pan">Pan between -1 (full left pan) and 1 (full right pan).</param>
        public void SetPan(double pan)
        {
            pan = Math.Max(pan, -1);
            pan = Math.Min(pan, 1);

            pan *= 0.5;
            pan += 0.48;
            
            SetPosition(2, pan);
        }

        /// <summary>
        /// Set the tilt of the camera.
        /// </summary>
        /// <param name="tilt">Tilt between -0.75 (down) and 1 (up).</param>
        public void SetTilt(double tilt)
        {
            tilt = Math.Max(tilt, -0.75);
            tilt = Math.Min(tilt, 1);

            tilt = -tilt;
            tilt *= 0.25;

            tilt += 0.67;

            SetPosition(3, tilt);
        }

        
    }
}
